#include <ros/ros.h>
#include <prometheus_msgs/UAVSetup.h>

using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "set_controller_mode");
    ros::NodeHandle nh("~");

    prometheus_msgs::UAVSetup uav_setup;
    ros::Publisher uav_setup_pub = nh.advertise<prometheus_msgs::UAVSetup>("/uav1/prometheus/setup", 10);
    std::string msg = "1 for land; 2 for command_control; 3 for arm; 4 for disarm; 5 for reboot px4; 0 for end\n";
    int key_in;
    while(true)
    {   
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -------- <<<<<<<<<<<<<<<<<<<<<<<<<<<" << endl;
        cout << msg << endl;
        cin >> key_in;
        if(cin.fail())
        {
            cout << "Input is not a Interge!" << endl;
            cin.clear();
            cin.ignore(numeric_limits<std::streamsize>::max(), '\n');
            continue;
        }

        if(key_in == 1)
        {
            uav_setup.cmd = prometheus_msgs::UAVSetup::SET_CONTROL_MODE;
            uav_setup.control_state = "LAND_CONTROL";
            uav_setup_pub.publish(uav_setup);
        }
        else if(key_in == 2)
        {
            uav_setup.cmd = prometheus_msgs::UAVSetup::SET_CONTROL_MODE;
            uav_setup.control_state = "COMMAND_CONTROL";
            uav_setup_pub.publish(uav_setup);
        }
        else if(key_in == 3)
        {
            uav_setup.cmd = prometheus_msgs::UAVSetup::ARMING;
            uav_setup.arming = true;
            uav_setup_pub.publish(uav_setup);
        }
        else if(key_in == 4)
        {
            uav_setup.cmd = prometheus_msgs::UAVSetup::ARMING;
            uav_setup.arming = false;
            uav_setup_pub.publish(uav_setup);
        }
        else if(key_in == 5)
        {
            uav_setup.cmd = prometheus_msgs::UAVSetup::REBOOT_PX4;
            uav_setup_pub.publish(uav_setup);
        }
        else if(key_in == 0)
        {
            break;
        }
        else
        {
            cout << "The input [" << key_in << "] doesn't match any situation!" << endl;
        }
    }

}